#include "MultiRobotClearanceDetector.h"
#include "Conversion.h"

MultiRobotClearanceDetector::MultiRobotClearanceDetector(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles)
    : m_robot_polygons(robot_polygons), m_robot_num(robot_num), m_obstacles(obstacles)
{
    std::cout << "[mr ClearanceD]\tInitializing " << m_robot_polygons.size() << " robots";
    BOOST_FOREACH(Polygon_2& robot, m_robot_polygons)
    {
        m_clearences.push_back(Clearance_detector_ie(obstacles, robot, 1));
    }
    std::cout << " - Done!\n";
}

double MultiRobotClearanceDetector::clearance(Point_d point)
{
    double value = 0;
    bool first = true;
    vector<Conf> points = Conversion::to_vector(point);
    vector<Conf>::iterator iter = points.begin();

    BOOST_FOREACH(Clearance_detector_ie& detector, m_clearences)
    {
        Conf c = *iter;
        double current = detector.clearance(c);
        if (first || (current < value))
        {
            first = false;
            value = current;
        }
        ++iter;
    }
    return value;
}
